#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <stdio.h>
#include <assert.h>
#ifdef DMALLOC
#include <imp/dmalloc.h>
#endif
typedef struct fifo_node_t{
	struct fifo_node_t *next;
	void  *pdata;
} fifo_node;

typedef struct {
	fifo_node *head;
	fifo_node *tail;
} fifo_list;

typedef struct {
	uint32_t magic;
	uint32_t maxcnt;
	uint32_t cnt;
	uint32_t datasize;
	fifo_list busy;
	fifo_list empty;
	fifo_node array[0];
} fifo_t;

#define FIFO_DATA_MAGIC (('F'<< 24)|('I'<< 16)|('F'<< 8)|('O'<< 0))
void* fifo_alloc(int cnt, int datasize)
{
	int i = 0;
	int size = 0;
	fifo_t *f;
	fifo_node *node;
	size = sizeof(fifo_t)+(sizeof(fifo_node)+datasize)*cnt;
	f = malloc(size);
	if (NULL == f) {
		printf("err(%s,%d): malloc err\n", __func__, __LINE__);
		return NULL;
	}
	memset(f, 0, size);
	node = &f->array[0];
	f->magic = FIFO_DATA_MAGIC;
	f->maxcnt = cnt;
	f->cnt = 0;
	node->pdata = (uint8_t *)f+sizeof(fifo_t)+sizeof(fifo_node)*cnt;
	f->empty.head = node;
	f->empty.tail = node;
	for (i = 1; i < cnt; i++) {
		fifo_node *node = &f->array[i];
		node->pdata = (uint8_t *)f+sizeof(fifo_t)+sizeof(fifo_node)*cnt+(i*datasize);
		f->empty.tail->next = node;
		f->empty.tail = node;
	}
	f->datasize = datasize;
	return f;
}

void fifo_free(void *fifo)
{
	fifo_t *f;
	f = (fifo_t*)fifo;
	if (FIFO_DATA_MAGIC != f->magic) {
		printf("err(%s,%d): fifo magic  err\n", __func__, __LINE__);
		return;
	}
	free(fifo);
	return;
}

int fifo_put(void *fifo, uint32_t *data)
{
	fifo_t *f;
	fifo_node *node;
	f = (fifo_t*)fifo;
	if (FIFO_DATA_MAGIC != f->magic) {
		printf("err(%s,%d): fifo magic  err\n", __func__, __LINE__);
		return -1;
	}
	if (NULL == f->empty.head) {
		printf("err(%s,%d): fifo empty null\n", __func__, __LINE__);
		return -1;
	}
	//get empty node
	node = f->empty.head;
	if (f->empty.head == f->empty.tail) {
		f->empty.head = NULL;
		f->empty.tail = NULL;
	} else {
		f->empty.head = node->next;
	}
	//put busy node
	memcpy(node->pdata, data, f->datasize);
	node->next = NULL;
	if ((NULL == f->busy.head)||(NULL == f->busy.tail)) {
		f->busy.head = node;
		f->busy.tail = node;
	} else {
		f->busy.tail->next = node;
		f->busy.tail = node;
	}
	f->cnt++;
	return 0;
}

int fifo_get(void *fifo, uint32_t *data)
{
	fifo_t *f = fifo;
	fifo_node *node;
	if (FIFO_DATA_MAGIC != f->magic) {
		printf("err(%s,%d): fifo magic  err\n", __func__, __LINE__);
		return -1;
	}
	if (NULL == f->busy.head) {
		//printf("warn(%s,%d): fifo busy null\n", __func__, __LINE__);
		return -1;
	}
	//get busy node
	node = f->busy.head;
	if (f->busy.head == f->busy.tail) {
		f->busy.head = NULL;
		f->busy.tail = NULL;
	} else {
		f->busy.head = node->next;
	}
	if (NULL != data)
		memcpy(data, node->pdata, f->datasize);
	memset(node->pdata, 0, f->datasize);
	node->next = NULL;
	//put empty node
	if ((NULL == f->empty.head)||(NULL == f->empty.tail)) {
		f->empty.head = node;
		f->empty.tail = node;
	} else {
		f->empty.tail->next = node;
		f->empty.tail = node;
	}
	f->cnt--;
	return 0;
}

int fifo_clear(void *fifo)
{
	fifo_t *f = fifo;
	fifo_node *node;
	if (FIFO_DATA_MAGIC != f->magic) {
		printf("err(%s,%d): fifo magic  err\n", __func__, __LINE__);
		return -1;
	}
	if (NULL == f->busy.head) {
		//printf("warn(%s,%d): fifo already empty\n", __func__, __LINE__);
		return 0;
	}
	//get busy node
	node = f->busy.head;
	while(node) {
		memset(node->pdata, 0, f->datasize);
		//put empty node
		if ((NULL == f->empty.head)||(NULL == f->empty.tail)) {
			f->empty.head = node;
			f->empty.tail = node;
		} else {
			f->empty.tail->next = node;
			f->empty.tail = node;
		}
		f->cnt--;
		node = node->next;
	}
	f->busy.head = NULL;
	f->busy.tail = NULL;
	return 0;
}

int fifo_pre_get(void *fifo, uint32_t *data)
{
	fifo_t *f = fifo;
	fifo_node *node;
	if (FIFO_DATA_MAGIC != f->magic) {
		printf("err(%s,%d): fifo magic  err\n", __func__, __LINE__);
		return -1;
	}
	if (NULL == f->busy.head) {
		printf("err(%s,%d): fifo busy null\n", __func__, __LINE__);
		return -1;
	}
	//get busy node
	node = f->busy.head;
	memcpy(data, node->pdata, f->datasize);
	return 0;
}

int fifo_pre_get_ptr(void *fifo, uint32_t idx, uint32_t **data)
{
	fifo_t *f = fifo;
	fifo_node *node;
	int i = 0;
	if (FIFO_DATA_MAGIC != f->magic) {
		printf("err(%s,%d): fifo magic  err\n", __func__, __LINE__);
		return -1;
	}
	if (NULL == f->busy.head) {
		//printf("warn(%s,%d): fifo busy null\n", __func__, __LINE__);
		return -1;
	}

	node = f->busy.head;
	if ((node)&&(idx == i)) {

		*data = node->pdata;
		return 0;
	}
	while(node) {
		node = node->next;
		i++;
		if ((node)&&(idx == i)) {
			*data = node->pdata;
			return 0;
		}
	}
	return -1;
}

int fifo_num(void *fifo)
{
	fifo_t *f = fifo;
	if (FIFO_DATA_MAGIC != f->magic) {
		printf("err(%s,%d): fifo magic  err\n", __func__, __LINE__);
		return -1;
	}
	return f->cnt;
}

void fifo_print(void *fifo)
{
	fifo_t *f = fifo;
	fifo_node *node;
	printf("----------------------\n");
	if (FIFO_DATA_MAGIC != f->magic) {
		printf("err(%s,%d): fifo magic  err\n", __func__, __LINE__);
		return;
	}
	printf("cnt = %d\n", f->cnt);
	node = f->busy.head;
	while(node) {
		printf("data = 0x%x\n", *(uint32_t*)(node->pdata));
		node = node->next;
	}
}

int fifo_head(void *fifo, void **node, void **data)
{
	fifo_t *f = fifo;
	fifo_node *n;
	if (FIFO_DATA_MAGIC != f->magic) {
		printf("err(%s,%d): fifo magic  err\n", __func__, __LINE__);
		return -1;
	}
	n = f->busy.head;
	*node = n;
	if (n) {
		*data = n->pdata;
	}
	return 0;
}

int fifo_node_next(void *fifo, void **node, void **data)
{
	fifo_t *f = fifo;
	fifo_node *n;
	if (FIFO_DATA_MAGIC != f->magic) {
		printf("err(%s,%d): fifo magic  err\n", __func__, __LINE__);
		return -1;
	}
	n = *node;
	if (n) {
		n = n->next;
		*node = n;
		if (n) {
			*data = n->pdata;
		}
	} else {
		return -1;
	}
	return 0;
}

#if 0
int fifo_find(void *fifo, int (*process)(void *arg, int idx, void *pdata), void *arg, void **pdata)
{
	fifo_t *f = fifo;
	fifo_node *node;
	if (FIFO_DATA_MAGIC != f->magic) {
		printf("err(%s,%d): fifo magic  err\n", __func__, __LINE__);
		*pdata = NULL;
		return -1;
	}
	node = f->busy.head;
	int stop = 0;
	int idx = 0;
	while(node) {
		stop = (*process)(arg, idx, node->pdata);
		node = node->next;
		idx++;
		if (stop) {
			*pdata = node->pdata;
			return 0;
		};
	}
	*pdata = NULL;
	return 0;
}
#endif

